//! Motor CRC calculation tests
//!
//! This module contains comprehensive tests for the motor CRC calculation functionality.

use unitree_ros2_rs::utils::motor_crc::*;
use r2r::unitree_go::msg::{LowCmd, MotorCmd, BmsCmd};

fn create_test_low_cmd() -> LowCmd {
    LowCmd {
        head: vec![0x55, 0xAA],
        level_flag: LOWLEVEL,
        frame_reserve: 0x00,
        sn: vec![0x12345678, 0x9ABCDEF0],
        version: vec![0x01020304, 0x05060708],
        bandwidth: 1000,
        motor_cmd: (0..20).map(|i| MotorCmd {
            mode: (i % 4) as u8,
            q: i as f32 * 0.1,
            dq: i as f32 * 0.01,
            tau: i as f32 * 0.001,
            kp: 100.0 + i as f32,
            kd: 10.0 + i as f32 * 0.1,
            reserve: vec![0x00000000, 0x11111111, 0x22222222],
        }).collect(),
        bms_cmd: BmsCmd {
            off: 0xA5,
            reserve: vec![0x00, 0x01, 0x02],
        },
        wireless_remote: vec![0; 40],
        led: vec![0xFF; 12],
        fan: vec![0x80, 0x40],
        gpio: 0x0F,
        reserve: 0xDEADBEEF,
        crc: 0, // Will be calculated
    }
}

#[test]
fn test_crc32_core_basic() {
    let data = [0x12345678u32, 0x9ABCDEF0u32];
    let result = crc32_core(&data);
    assert!(result.is_ok());
    
    // Test with empty slice
    let empty: &[u32] = &[];
    let result = crc32_core(empty);
    assert!(matches!(result, Err(CrcError::InvalidInput(_))));
}

#[test]
fn test_crc32_core_deterministic() {
    let data = [0x12345678u32, 0x9ABCDEF0u32, 0xFEDCBA98u32];
    let result1 = crc32_core(&data).unwrap();
    let result2 = crc32_core(&data).unwrap();
    assert_eq!(result1, result2, "CRC calculation should be deterministic");
}

#[test]
fn test_get_crc_success() {
    let mut cmd = create_test_low_cmd();
    let result = get_crc(&mut cmd);
    assert!(result.is_ok());
    assert_ne!(cmd.crc, 0, "CRC should be calculated and non-zero");
}

#[test]
fn test_get_crc_deterministic() {
    let mut cmd1 = create_test_low_cmd();
    let mut cmd2 = create_test_low_cmd();
    
    get_crc(&mut cmd1).unwrap();
    get_crc(&mut cmd2).unwrap();
    
    assert_eq!(cmd1.crc, cmd2.crc, "Identical inputs should produce identical CRC");
}

#[test]
fn test_get_crc_different_inputs() {
    let mut cmd1 = create_test_low_cmd();
    let mut cmd2 = create_test_low_cmd();
    cmd2.level_flag = HIGHLEVEL; // Change one field
    
    get_crc(&mut cmd1).unwrap();
    get_crc(&mut cmd2).unwrap();
    
    assert_ne!(cmd1.crc, cmd2.crc, "Different inputs should produce different CRC");
}

#[test]
fn test_input_validation() {
    // Test invalid motor_cmd array length
    let mut cmd = create_test_low_cmd();
    cmd.motor_cmd = vec![]; // Empty array
    let result = get_crc(&mut cmd);
    assert!(matches!(result, Err(CrcError::InvalidInput(_))));
    
    // Test invalid head array length
    let mut cmd = create_test_low_cmd();
    cmd.head = vec![0x55]; // Wrong size
    let result = get_crc(&mut cmd);
    assert!(matches!(result, Err(CrcError::InvalidInput(_))));
}

#[test]
fn test_endianness_consistency() {
    // This test ensures that the native endian approach works consistently
    let mut cmd = create_test_low_cmd();
    
    // Set specific values that would differ between little and big endian
    cmd.sn = vec![0x12345678, 0x9ABCDEF0];
    cmd.version = vec![0x01020304, 0x05060708];
    cmd.bandwidth = 0x1234;
    cmd.reserve = 0xDEADBEEF;
    
    let result = get_crc(&mut cmd);
    assert!(result.is_ok());
    
    // The CRC should be calculated consistently regardless of platform
    // This is a regression test to catch endianness issues
    let crc_value = cmd.crc;
    assert_ne!(crc_value, 0);
    
    // Run again to ensure consistency
    cmd.crc = 0;
    get_crc(&mut cmd).unwrap();
    assert_eq!(cmd.crc, crc_value);
}

#[test]
fn test_constants_values() {
    assert_eq!(HIGHLEVEL, 0xee);
    assert_eq!(LOWLEVEL, 0xff);
    assert_eq!(TRIGERLEVEL, 0xf0);
    assert_eq!(POS_STOP_F, 2.146E+9);
    assert_eq!(VEL_STOP_F, 16000.0);
    
    // Test joint indices
    assert_eq!(FR_0, 0);
    assert_eq!(FR_1, 1);
    assert_eq!(FR_2, 2);
    assert_eq!(FL_0, 3);
    assert_eq!(RL_2, 11);
}

#[test]
fn test_performance_stack_allocation() {
    // This test ensures that our stack allocation approach works
    // and doesn't cause stack overflow for normal inputs
    let mut cmd = create_test_low_cmd();
    
    // Run multiple times to test stack usage
    for _ in 0..100 {
        cmd.crc = 0;
        let result = get_crc(&mut cmd);
        assert!(result.is_ok());
    }
}